package com.tjvib.sensor;

import static com.tjvib.Constants.SUCCESS;

import android.bluetooth.BluetoothAdapter;
import android.bluetooth.BluetoothDevice;
import android.bluetooth.BluetoothSocket;
import android.content.Context;

import com.tjvib.bean.DataSet;
import com.tjvib.bean.DataSetInfo;
import com.tjvib.common.Callback;
import com.tjvib.util.FileUtil;
import com.tjvib.util.LogUtil;
import com.tjvib.util.Manager.AppManager;
import com.tjvib.util.Manager.UserManager;
import com.tjvib.util.ToastUtil;
import com.tjvib.util.retrofit.BaseResponse;
import com.tjvib.util.retrofit.RetrofitHelper;
import com.tjvib.view.activity.LpmsB2SensorActivity;

import org.json.JSONException;

import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.nio.ByteBuffer;
import java.sql.Timestamp;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.UUID;

import io.reactivex.Observer;
import io.reactivex.disposables.Disposable;
import okhttp3.MediaType;
import okhttp3.RequestBody;

public class LpmsB2Sensor extends Sensor<LpmsB2SensorData> {

    private static Context context;
    //////////////////////////////////////////////
    // Stream frequency enable bits
    //////////////////////////////////////////////
    public static final int LPMS_FILTER_GYR = 0;
    public static final int LPMS_FILTER_GYR_ACC = 1;
    public static final int LPMS_FILTER_GYR_ACC_MAG = 2;
    public static final int LPMS_FILTER_MADGWICK_GYR_ACC = 3;
    public static final int LPMS_FILTER_MADGWICK_GYR_ACC_MAG = 4;
    // Gyro Range
    public static final int LPMS_GYR_RANGE_125DPS = 125;
    public static final int LPMS_GYR_RANGE_245DPS = 245;
    public static final int LPMS_GYR_RANGE_250DPS = 250;
    public static final int LPMS_GYR_RANGE_500DPS = 500;
    public static final int LPMS_GYR_RANGE_1000DPS = 1000;
    public static final int LPMS_GYR_RANGE_2000DPS = 2000;
    // Acc Range
    public static final int LPMS_ACC_RANGE_2G = 2;
    public static final int LPMS_ACC_RANGE_4G = 4;
    public static final int LPMS_ACC_RANGE_8G = 8;
    public static final int LPMS_ACC_RANGE_16G = 16;
    // Mag Range
    public static final int LPMS_MAG_RANGE_4GAUSS = 4;
    public static final int LPMS_MAG_RANGE_8GAUSS = 8;
    public static final int LPMS_MAG_RANGE_12GAUSS = 12;
    public static final int LPMS_MAG_RANGE_16GAUSS = 16;
    public static final int PARAMETER_SET_DELAY = 10;
    // Offset mode
    public static final int LPMS_OFFSET_MODE_OBJECT = 0;
    public static final int LPMS_OFFSET_MODE_HEADING = 1;
    public static final int LPMS_OFFSET_MODE_ALIGNMENT = 2;

    final String TAG = "LpmsB2";
    final UUID MY_UUID_INSECURE = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB");
    final int PACKET_ADDRESS0 = 0;
    final int PACKET_ADDRESS1 = 1;
    final int PACKET_FUNCTION0 = 2;
    final int PACKET_FUNCTION1 = 3;
    final int PACKET_LENGTH0 = 4;
    final int PACKET_LENGTH1 = 5;
    final int PACKET_RAW_DATA = 6;
    final int PACKET_LRC_CHECK0 = 7;
    final int PACKET_LRC_CHECK1 = 8;
    final int PACKET_END = 9;

    //////////////////////////////////////////////
    // Command Registers
    //////////////////////////////////////////////
    final int REPLY_ACK = 0;
    final int REPLY_NACK = 1;
    final int UPDATE_FIRMWARE = 2;
    final int UPDATE_IAP = 3;
    final int GET_CONFIG = 4;
    final int GET_STATUS = 5;
    final int GOTO_COMMAND_MODE = 6;
    final int GOTO_STREAM_MODE = 7;
    final int GET_SENSOR_DATA = 9;
    final int SET_TRANSMIT_DATA = 10;
    final int SET_STREAM_FREQ = 11;
    // Register value save and reset
    final int WRITE_REGISTERS = 15;
    final int RESTORE_FACTORY_VALUE = 16;
    // Reference setting and offset reset
    final int RESET_REFERENCE = 17;
    final int SET_ORIENTATION_OFFSET = 18;
    final int RESET_ORIENTATION_OFFSET = 82;
    //IMU ID setting
    final int SET_IMU_ID = 20;
    final int GET_IMU_ID = 21;
    // Gyroscope settings
    final int START_GYR_CALIBRA = 22;
    final int ENABLE_GYR_AUTOCAL = 23;
    final int ENABLE_GYR_THRES = 24;
    final int SET_GYR_RANGE = 25;
    final int GET_GYR_RANGE = 26;
    // Accelerometer settings
    final int SET_ACC_BIAS = 27;
    final int GET_ACC_BIAS = 28;
    final int SET_ACC_ALIGN_MATRIX = 29;
    final int GET_ACC_ALIGN_MATRIX = 30;
    final int SET_ACC_RANGE = 31;
    final int GET_ACC_RANGE = 32;
    final int SET_GYR_ALIGN_BIAS = 48;
    final int GET_GYR_ALIGN_BIAS = 49;
    final int SET_GYR_ALIGN_MATRIX = 50;
    final int GET_GYR_ALIGN_MATRIX = 51;
    // Magnetometer settings
    final int SET_MAG_RANGE = 33;
    final int GET_MAG_RANGE = 34;
    final int SET_HARD_IRON_OFFSET = 35;
    final int GET_HARD_IRON_OFFSET = 36;
    final int SET_SOFT_IRON_MATRIX = 37;
    final int GET_SOFT_IRON_MATRIX = 38;
    final int SET_FIELD_ESTIMATE = 39;
    final int GET_FIELD_ESTIMATE = 40;
    final int SET_MAG_ALIGNMENT_MATRIX = 76;
    final int SET_MAG_ALIGNMENT_BIAS = 77;
    final int SET_MAG_REFRENCE = 78;
    final int GET_MAG_ALIGNMENT_MATRIX = 79;
    final int GET_MAG_ALIGNMENT_BIAS = 80;
    final int GET_MAG_REFERENCE = 81;
    // Filter settings
    final int SET_FILTER_MODE = 41;
    final int GET_FILTER_MODE = 42;
    final int SET_FILTER_PRESET = 43;
    final int GET_FILTER_PRESET = 44;
    final int SET_LIN_ACC_COMP_MODE = 67;
    final int GET_LIN_ACC_COMP_MODE = 68;

    //////////////////////////////////////////////
    // Status register contents
    //////////////////////////////////////////////
    final int SET_CENTRI_COMP_MODE = 69;
    final int GET_CENTRI_COMP_MODE = 70;
    final int SET_RAW_DATA_LP = 60;
    final int GET_RAW_DATA_LP = 61;
    final int SET_TIME_STAMP = 66;
    final int SET_LPBUS_DATA_MODE = 75;
    final int GET_FIRMWARE_VERSION = 47;
    final int GET_BATTERY_LEVEL = 87;
    final int GET_BATTERY_VOLTAGE = 88;
    final int GET_CHARGING_STATUS = 89;
    final int GET_SERIAL_NUMBER = 90;
    final int GET_DEVICE_NAME = 91;
    final int GET_FIRMWARE_INFO = 92;
    final int START_SYNC = 96;
    final int STOP_SYNC = 97;
    final int GET_PING = 98;
    final int GET_TEMPERATURE = 99;

    //////////////////////////////////////////////
    // Configuration register contents
    //////////////////////////////////////////////
    public static final int LPMS_GYR_AUTOCAL_ENABLED = 0x00000001 << 30;
    public static final int LPMS_LPBUS_DATA_MODE_16BIT_ENABLED = 0x00000001 << 22;
    public static final int LPMS_LINACC_OUTPUT_ENABLED = 0x00000001 << 21;
    public static final int LPMS_DYNAMIC_COVAR_ENABLED = 0x00000001 << 20;
    public static final int LPMS_ALTITUDE_OUTPUT_ENABLED = 0x00000001 << 19;
    public static final int LPMS_QUAT_OUTPUT_ENABLED = 0x00000001 << 18;
    public static final int LPMS_EULER_OUTPUT_ENABLED = 0x00000001 << 17;
    public static final int LPMS_ANGULAR_VELOCITY_OUTPUT_ENABLED = 0x00000001 << 16;
    public static final int LPMS_GYR_CALIBRA_ENABLED = 0x00000001 << 15;
    public static final int LPMS_HEAVEMOTION_OUTPUT_ENABLED = 0x00000001 << 14;
    public static final int LPMS_TEMPERATURE_OUTPUT_ENABLED = 0x00000001 << 13;
    public static final int LPMS_GYR_RAW_OUTPUT_ENABLED = 0x00000001 << 12;
    public static final int LPMS_ACC_RAW_OUTPUT_ENABLED = 0x00000001 << 11;
    public static final int LPMS_MAG_RAW_OUTPUT_ENABLED = 0x00000001 << 10;
    public static final int LPMS_PRESSURE_OUTPUT_ENABLED = 0x00000001 << 9;
    final int LPMS_STREAM_FREQ_5HZ_ENABLED = 0x00000000;
    final int LPMS_STREAM_FREQ_10HZ_ENABLED = 0x00000001;
    final int LPMS_STREAM_FREQ_25HZ_ENABLED = 0x00000002;
    final int LPMS_STREAM_FREQ_50HZ_ENABLED = 0x00000003;
    final int LPMS_STREAM_FREQ_100HZ_ENABLED = 0x00000004;
    final int LPMS_STREAM_FREQ_200HZ_ENABLED = 0x00000005;
    final int LPMS_STREAM_FREQ_400HZ_ENABLED = 0x00000006;
    final int LPMS_STREAM_FREQ_MASK = 0x00000007;

    final int MAX_BUFFER = 512;
    final int DATA_QUEUE_SIZE = 64;

    // Protocol parsing related
    int rxState = PACKET_END;
    byte[] rxBuffer = new byte[MAX_BUFFER];
    byte[] txBuffer = new byte[MAX_BUFFER];
    byte[] rawTxData = new byte[MAX_BUFFER];
    byte[] rawRxBuffer = new byte[MAX_BUFFER];
    int currentAddress = 0;
    int currentFunction = 0;
    int currentLength = 0;
    int rxIndex = 0;
    byte b = 0;
    int lrcCheck = 0;
    int nBytes = 0;
    boolean waitForAck = false;
    boolean waitForData = false;
    byte inBytes[] = new byte[2];
    // Connection related
    BluetoothDevice mDevice;
    String mAddress;
    InputStream mInStream;
    OutputStream mOutStream;
    BluetoothSocket mSocket;
    BluetoothAdapter mAdapter;

    // Settings related
    int imuId = 0;
    int gyrRange = 0;
    int accRange = 0;
    int magRange = 0;
    int streamingFrequency = 0;
    int filterMode = 0;
    boolean isStreamMode = false;

    int configurationRegister = 0;
    private boolean configurationRegisterReady = false;
    private String serialNumber = "";
    private boolean serialNumberReady = false;
    private String deviceName = "";
    private boolean deviceNameReady = false;
    private String firmwareInfo = "";
    private boolean firmwareInfoReady = false;
    private String firmwareVersion;

    boolean accEnable = true;
    boolean gyrEnable = true;
    boolean magEnable = true;
    boolean angularVelEnable = true;
    boolean quaternionEnable = true;
    boolean eulerAngleEnable = true;
    boolean linAccEnable = true;
    boolean pressureEnable = true;
    boolean altitudeEnable = true;
    boolean temperatureEnable = true;
    boolean heaveEnable = true;
    boolean sixteenBitDataEnable = false;
    boolean resetTimestampFlag = false;

    boolean newDataFlag = false;
    int frameCounter = 0;

    public static boolean isRecording = false;
    LpmsB2SensorData currentData = new LpmsB2SensorData();

    private static ArrayList<LpmsB2SensorData> records = new ArrayList<>();

    private static HashMap<Integer, Boolean> initMap = new HashMap<>();

    public LpmsB2Sensor(String name, String mAddress) {
        mAdapter = BluetoothAdapter.getDefaultAdapter();
        this.deviceName = name;
        this.mAddress = mAddress;
    }

    /////////////////////////////////////////////////////////////////////
    // Public methods
    /////////////////////////////////////////////////////////////////////

    /**
     * @return false or true
     * if true , the sensor will be at command mode.And after that, you should
     * use setAcquisitionParameters() to set some  sensor param.
     */
    @Override
    public boolean connect() {
        //LogUtil.d("[LpmsBThread] Another connection establishing: " +address);
        if (connectionStatus != SENSOR_STATUS_DISCONNECTED) {
            //LogUtil.d("[LpmsBThread] Another connection establishing: " + mAddress);
            return false;
        }

        if (mAdapter == null) {
            //LogUtil.d("[LpmsBThread] Didn't find Bluetooth adapter");
            connectionStatus = SENSOR_STATUS_DISCONNECTED;
            return false;
        }

        connectionStatus = SENSOR_STATUS_CONNECTING;
        //LogUtil.d("[LpmsBThread] Connecting to: " + mAddress);

        mAdapter.cancelDiscovery();

        try {
            mDevice = mAdapter.getRemoteDevice(mAddress);
        } catch (IllegalArgumentException e) {
            //LogUtil.d("[LpmsBThread] Invalid Bluetooth address", e);
            connectionStatus = SENSOR_STATUS_DISCONNECTED;
            return false;
        }

        mSocket = null;
        //LogUtil.d("[LpmsBThread] Creating socket");

        try {
            mSocket = mDevice.createInsecureRfcommSocketToServiceRecord(MY_UUID_INSECURE);
        } catch (Exception e) {
            //LogUtil.d("[LpmsBThread] Socket create() failed", e);
            connectionStatus = SENSOR_STATUS_DISCONNECTED;
            return false;
        }

        //LogUtil.d("[LpmsBThread] Trying to connect..");
        try {
            mSocket.connect();
        } catch (IOException e) {
            //LogUtil.d("[LpmsBThread] Couldn't connect to device", e);
            connectionStatus = SENSOR_STATUS_DISCONNECTED;
            return false;
        }

        //LogUtil.d("[LpmsBThread] Connected!");

        try {
            mInStream = mSocket.getInputStream();
            mOutStream = mSocket.getOutputStream();

        } catch (IOException e) {
            //LogUtil.d("[LpmsBThread] Streams not created", e);
            connectionStatus = SENSOR_STATUS_DISCONNECTED;
            return false;
        }


        Thread t = new Thread(new ClientReadThread());
        t.start();

        connectionStatus = SENSOR_STATUS_CONNECTED;

        if(LpmsB2SensorActivity.firstTimestamp == 0)
            LpmsB2SensorActivity.firstTimestamp = System.currentTimeMillis();

        setCommandMode();
        _getSerialNumber();
        _getDeviceName();
        _getFirmwareInfo();
        _getSensorSettings();
        setStreamingMode();
        frameCounter = 0;
        return true;
    }

    /**
     * disconnect cancel discovery and socket close.
     *
     * @return
     */
    @Override
    public void disconnect() {
        mAdapter.cancelDiscovery();
        try {
            mSocket.close();
        } catch (Exception e) {
            e.printStackTrace();
        }

        // clean up
        // dataQueue.clear();
        //stop/Logging();
        //LogUtil.d("[LpmsBThread] Connection closed test");
        connectionStatus = SENSOR_STATUS_DISCONNECTED;
    }

    public String getAddress() {
        return mAddress;
    }

    public BluetoothDevice getBluetoothDevice() {
        return mDevice;
    }

    /**
     * use command mode to communicate with sensor.
     */
    public void setCommandMode() {
        if (!assertConnected())
            return;
        waitForAck = true;
        lpbusSetNone(GOTO_COMMAND_MODE);
        //LogUtil.d("Send GOTO_COMMAND_MODE");
        _waitForAckLoop();
        isStreamMode = false;
    }

    /**
     * change sensor to Streaming mode.
     * it will send data too much ,and you have better to change sensor mode to set sensor or get some data you need.
     */
    public void setStreamingMode() {
        if (!assertConnected())
            return;
        waitForAck = true;
        lpbusSetNone(GOTO_STREAM_MODE);
        //LogUtil.d("Send GOTO_STREAM_MODE");
        _waitForAckLoop();
        isStreamMode = true;
    }

    public void setImuId(final int id) {
        if (!assertConnected())
            return;

        new Thread(new Thread() {
            public void run() {
                boolean b = isStreamMode;
                setCommandMode();
                waitForAck = true;
                lpbusSetInt32(SET_IMU_ID, id);
                _waitForAckLoop();
                _getSensorSettings();
                _saveParameters();
                if (b)
                    setStreamingMode();
            }
        }).start();
    }

    public int getImuId() {
        return imuId;
    }

    public int getGyroRange() {
        return gyrRange;
    }

    public void setGyroRange(final int range) {
        if (!assertConnected())
            return;
        if (range == LPMS_GYR_RANGE_125DPS ||
                range == LPMS_GYR_RANGE_245DPS ||
                range == LPMS_GYR_RANGE_500DPS ||
                range == LPMS_GYR_RANGE_1000DPS ||
                range == LPMS_GYR_RANGE_2000DPS) {

            new Thread(new Thread() {
                public void run() {
                    boolean b = isStreamMode;
                    setCommandMode();
                    waitForAck = true;
                    lpbusSetInt32(SET_GYR_RANGE, range);
                    _waitForAckLoop();
                    _getSensorSettings();
                    _saveParameters();
                    if (b)
                        setStreamingMode();
                }
            }).start();
        }
    }

    public int getAccRange() {
        return accRange;
    }

    public void setAccRange(final int range) {
        if (!assertConnected())
            return;
        if (range == LPMS_ACC_RANGE_2G ||
                range == LPMS_ACC_RANGE_4G ||
                range == LPMS_ACC_RANGE_8G ||
                range == LPMS_ACC_RANGE_16G) {

            new Thread(new Thread() {
                public void run() {
                    boolean b = isStreamMode;
                    setCommandMode();
                    waitForAck = true;
                    lpbusSetInt32(SET_ACC_RANGE, range);
                    _waitForAckLoop();
                    _getSensorSettings();
                    _saveParameters();
                    if (b)
                        setStreamingMode();
                }
            }).start();
        }
    }


    public int getMagRange() {
        return magRange;
    }

    public void setMagRange(final int range) {
        if (!assertConnected())
            return;
        if (range == LPMS_MAG_RANGE_4GAUSS ||
                range == LPMS_MAG_RANGE_8GAUSS ||
                range == LPMS_MAG_RANGE_12GAUSS ||
                range == LPMS_MAG_RANGE_16GAUSS) {

            new Thread(new Thread() {
                public void run() {
                    boolean b = isStreamMode;
                    setCommandMode();
                    waitForAck = true;
                    lpbusSetInt32(SET_MAG_RANGE, range);
                    _waitForAckLoop();
                    _getSensorSettings();
                    _saveParameters();
                    if (b)
                        setStreamingMode();
                }
            }).start();
        }
    }


    public int getFilterMode() {
        return filterMode;
    }

    public void setFilterMode(final int mode) {
        if (!assertConnected())
            return;
        if (mode == LPMS_FILTER_GYR ||
                mode == LPMS_FILTER_GYR_ACC ||
                mode == LPMS_FILTER_GYR_ACC_MAG ||
                mode == LPMS_FILTER_MADGWICK_GYR_ACC ||
                mode == LPMS_FILTER_MADGWICK_GYR_ACC_MAG) {

            new Thread(new Thread() {
                public void run() {
                    boolean b = isStreamMode;
                    setCommandMode();
                    waitForAck = true;
                    lpbusSetInt32(SET_FILTER_MODE, mode);
                    _waitForAckLoop();
                    _getSensorSettings();
                    _saveParameters();
                    if (b)
                        setStreamingMode();
                }
            }).start();
        }
    }

    @Override
    public void setStreamFrequency(int freq) {
        if (!assertConnected())
            return;
        if (freq == STREAM_FREQ_5HZ ||
                freq == STREAM_FREQ_10HZ ||
                freq == STREAM_FREQ_25HZ ||
                freq == STREAM_FREQ_50HZ ||
                freq == STREAM_FREQ_100HZ ||
                freq == STREAM_FREQ_200HZ ||
                freq == STREAM_FREQ_400HZ) {

            new Thread(new Thread() {
                public void run() {
                    boolean b = isStreamMode;
                    setCommandMode();
                    waitForAck = true;
                    lpbusSetInt32(SET_STREAM_FREQ, freq);
                    _waitForAckLoop();
                    _getSensorSettings();
                    _saveParameters();
                    if (b)
                        setStreamingMode();
                }
            }).start();
        }
    }

    public void setTransmissionData(final int v) {
        if (!assertConnected())
            return;

        new Thread(new Thread() {
            public void run() {
                boolean b = isStreamMode;
                setCommandMode();
                waitForAck = true;
                lpbusSetInt32(SET_TRANSMIT_DATA, v);
                _waitForAckLoop();
                _getSensorSettings();
                _saveParameters();
                if (b)
                    setStreamingMode();
            }
        }).start();

    }

    public void enableGyroData(boolean b) {
        if (!assertConnected())
            return;
        if (b)
            configurationRegister |= LPMS_GYR_RAW_OUTPUT_ENABLED;
        else
            configurationRegister &= ~LPMS_GYR_RAW_OUTPUT_ENABLED;
        _setTransmissionData();
    }

    public boolean isGyroDataEnabled() {
        return gyrEnable;
    }

    public void enableAccData(boolean b) {
        if (!assertConnected())
            return;
        if (b)
            configurationRegister |= LPMS_ACC_RAW_OUTPUT_ENABLED;
        else
            configurationRegister &= ~LPMS_ACC_RAW_OUTPUT_ENABLED;
        _setTransmissionData();
    }

    public boolean isAccDataEnabled() {
        return accEnable;
    }

    public void enableMagData(boolean b) {
        if (!assertConnected())
            return;
        if (b)
            configurationRegister |= LPMS_MAG_RAW_OUTPUT_ENABLED;
        else
            configurationRegister &= ~LPMS_MAG_RAW_OUTPUT_ENABLED;
        _setTransmissionData();
    }

    public boolean isMagDataEnabled() {
        return magEnable;
    }

    public void enableAngularVelData(boolean b) {
        if (!assertConnected())
            return;
        if (b)
            configurationRegister |= LPMS_ANGULAR_VELOCITY_OUTPUT_ENABLED;
        else
            configurationRegister &= ~LPMS_ANGULAR_VELOCITY_OUTPUT_ENABLED;
        _setTransmissionData();
    }

    public boolean isAngularVelDataEnable() {
        return angularVelEnable;
    }

    public void enableQuaternionData(boolean b) {
        if (!assertConnected())
            return;
        if (b)
            configurationRegister |= LPMS_QUAT_OUTPUT_ENABLED;
        else
            configurationRegister &= ~LPMS_QUAT_OUTPUT_ENABLED;
        _setTransmissionData();
    }

    public boolean isQuaternionDataEnabled() {
        return quaternionEnable;
    }

    public void enableEulerData(boolean b) {
        if (!assertConnected())
            return;
        if (b)
            configurationRegister |= LPMS_EULER_OUTPUT_ENABLED;
        else
            configurationRegister &= ~LPMS_EULER_OUTPUT_ENABLED;
        _setTransmissionData();
    }

    public boolean isEulerDataEnabled() {
        return eulerAngleEnable;
    }

    public void enableLinAccData(boolean b) {
        if (!assertConnected())
            return;
        if (b)
            configurationRegister |= LPMS_LINACC_OUTPUT_ENABLED;
        else
            configurationRegister &= ~LPMS_LINACC_OUTPUT_ENABLED;
        _setTransmissionData();
    }

    public boolean isLinAccDataEnabled() {
        return linAccEnable;
    }

    public void enablePressureData(boolean b) {
        if (!assertConnected())
            return;
        if (b)
            configurationRegister |= LPMS_PRESSURE_OUTPUT_ENABLED;
        else
            configurationRegister &= ~LPMS_PRESSURE_OUTPUT_ENABLED;
        _setTransmissionData();
    }

    public boolean isPressureDataEnabled() {
        return pressureEnable;
    }

    public void enableAltitudeData(boolean b) {
        if (!assertConnected())
            return;
        if (b)
            configurationRegister |= LPMS_ALTITUDE_OUTPUT_ENABLED;
        else
            configurationRegister &= ~LPMS_ALTITUDE_OUTPUT_ENABLED;
        _setTransmissionData();
    }

    public boolean isAltitudeDataEnabled() {
        return altitudeEnable;
    }


    public void enableTemperatureData(boolean b) {
        if (!assertConnected())
            return;
        if (b)
            configurationRegister |= LPMS_TEMPERATURE_OUTPUT_ENABLED;
        else
            configurationRegister &= ~LPMS_TEMPERATURE_OUTPUT_ENABLED;
        _setTransmissionData();
    }

    public boolean isTemperatureDataEnabled() {
        return temperatureEnable;
    }

    public void enable16BitData() {
        if (!assertConnected())
            return;
        configurationRegister |= LPMS_LPBUS_DATA_MODE_16BIT_ENABLED;
        _setTransmissionData();
    }

    public void enable32BitData() {
        if (!assertConnected())
            return;
        configurationRegister &= ~LPMS_LPBUS_DATA_MODE_16BIT_ENABLED;
        _setTransmissionData();
    }

    public boolean is16BitDataEnabled() {
        if (sixteenBitDataEnable)
            return true;
        else
            return false;
    }

    @Override
    public LpmsB2SensorData getData() {
        LpmsB2SensorData d = null;
        if (!assertConnected())
            return d;
        if (!isStreamMode) {

            synchronized (dataQueue) {
                while (dataQueue.size() > 0) {
                    d = dataQueue.getLast();
                    dataQueue.removeLast();
                }
            }
            waitForData = true;
            lpbusSetNone(GET_SENSOR_DATA);
            //LogUtil.d("Send GET_SENSOR_DATA");
            _waitForDataLoop();
        }
        synchronized (dataQueue) {
            if (dataQueue.size() > 0) {
                d = dataQueue.getLast();
                dataQueue.removeLast();
            }
        }

        return d;
    }

    public String getSerialNumber() {
        return serialNumber;
    }


    public String getDeviceName() {
        return deviceName;
    }


    public boolean isStreamingMode() {
        return isStreamMode;
    }

    public String getFirmwareInfo() {
        return firmwareInfo;
    }

    public void startSyncSensor() {
        if (!assertConnected())
            return;
        lpbusSetNone(START_SYNC);
        waitForAck = true;
        _waitForAckLoop();
    }

    public void stopSyncSensor() {
        if (!assertConnected())
            return;
        lpbusSetNone(STOP_SYNC);
        waitForAck = true;
        _waitForAckLoop();
    }

    private void testPing() {
        if (!assertConnected())
            return;
        lpbusSetNone(GET_PING);
        //LogUtil.d("Send GET_PING");
    }

    public void resetFactorySettings() {
        if (!assertConnected())
            return;

        boolean b = isStreamMode;
        setCommandMode();
        waitForAck = true;
        lpbusSetNone(RESTORE_FACTORY_VALUE);
        _waitForAckLoop();
        _getSensorSettings();
        _saveParameters();
        if (b)
            setStreamingMode();
    }

    public void setOrientationOffset(int offset) {

        if (!assertConnected())
            return;
        if (offset == LPMS_OFFSET_MODE_ALIGNMENT ||
                offset == LPMS_OFFSET_MODE_HEADING ||
                offset == LPMS_OFFSET_MODE_OBJECT) {
            lpbusSetInt32(SET_ORIENTATION_OFFSET, offset);
        }
    }

    public void resetOrientationOffset() {
        if (!assertConnected())
            return;
        lpbusSetNone(RESET_ORIENTATION_OFFSET);
    }

    @Override
    public void resetTimestamp() {
        if (!assertConnected())
            return;
        lpbusSetInt32(SET_TIME_STAMP, 0);
    }

    public static void discardData() {
        records.clear();
    }

    public static void storeData(String name, Callback callback) {
        DataSetInfo info = new DataSetInfo(-1, name, "lpmsb2", false, new Timestamp(System.currentTimeMillis()).toString());
        DataSet<LpmsB2SensorData> dataSet = new DataSet<>(info, records);
        new Thread(() -> {
            String dataSetStr = null;
            try {
                dataSetStr = dataSet.toJsonString();
            } catch (JSONException e) {
                e.printStackTrace();
                LogUtil.i("json=" + e.getMessage());
            }
            RequestBody requestBody = RequestBody.create(MediaType.parse("application/json"), dataSetStr);
            RetrofitHelper.getInstance().create()
                    .collectLpmsB2("" + UserManager.getInstance().getUserId(), requestBody)
                    .subscribe(new Observer<BaseResponse>() {
                        @Override
                        public void onSubscribe(Disposable d) {
                            LogUtil.e("onSubscribe "+d);
                        }

                        @Override
                        public void onNext(BaseResponse value) {
                            if (value.getCode() == SUCCESS) {
                                callback.onSuccess(value.getMessage(), null);
                                FileUtil.deleteDataSetTempFile(context);
                            } else {
                                callback.onFail(value.getMessage(), null);
                            }
                            LogUtil.i("collectLpmsB2 "+value.getCode() + " " + value.getMessage() + " " + value.getData());
                        }

                        @Override
                        public void onError(Throwable e) {
                            if (e != null) {
                                ToastUtil.show(e.getMessage());
                                LogUtil.e("e=" + e.getMessage());
                            } else {
                                LogUtil.e("null");
                            }
                        }

                        @Override
                        public void onComplete() {
                            LogUtil.e("onComplete");
                        }
                    });
        }).start();
    }

    public void setTimestamp(int ts) {
        if (!assertConnected())
            return;
        lpbusSetInt32(SET_TIME_STAMP, ts);
    }

    public void getBatteryPercentage() {
        if (!assertConnected())
            return;
        lpbusSetNone(GET_BATTERY_LEVEL);
        //LogUtil.d("Send  GET_BATTERY_LEVEL");
    }

    public void getBatteryVoltage() {
        if (!assertConnected())
            return;
        lpbusSetNone(GET_BATTERY_VOLTAGE);
        //LogUtil.d("Send  GET_BATTERY_VOLTAGE");
    }

    public void getChargingStatus() {
        if (!assertConnected())
            return;
        lpbusSetNone(GET_CHARGING_STATUS);
        //LogUtil.d("Send  GET_CHARGING_STATUS");
    }

    /////////////////////////////////////////////////////////////////////
    // LP Bus Related
    /////////////////////////////////////////////////////////////////////
    void lpbusSetInt32(int command, int v) {
        for (int i = 0; i < 4; ++i) {
            rawTxData[i] = (byte) (v & 0xff);
            v = v >> 8;
        }

        sendData(command, 4);
    }

    void lpbusSetNone(int command) {
        sendData(command, 0);
    }

    void lpbusSetData(int command, int length, byte[] dataBuffer) {
        for (int i = 0; i < length; ++i) {
            rawTxData[i] = dataBuffer[i];
        }

        sendData(command, length);
    }

    void parseSensorData() {
        int o = 0;
        float r2d = 57.2958f;

        currentData.imuId = imuId;
        currentData.timestamp = (float) convertRxbytesToInt(o, rxBuffer) * 0.0025f;
        currentData.localtime = System.currentTimeMillis();
        o += 4;
        if (gyrEnable) {
            currentData.gyr[0] = convertRxbytesToFloat(o, rxBuffer) * r2d;
            o += 4;
            currentData.gyr[1] = convertRxbytesToFloat(o, rxBuffer) * r2d;
            o += 4;
            currentData.gyr[2] = convertRxbytesToFloat(o, rxBuffer) * r2d;
            o += 4;
        }

        if (accEnable) {
            currentData.acc[0] = (convertRxbytesToFloat(o, rxBuffer));
            o += 4;
            currentData.acc[1] = (convertRxbytesToFloat(o, rxBuffer));
            o += 4;
            currentData.acc[2] = (convertRxbytesToFloat(o, rxBuffer));
            o += 4;
        }

        if (magEnable) {
            currentData.mag[0] = convertRxbytesToFloat(o, rxBuffer);
            o += 4;
            currentData.mag[1] = convertRxbytesToFloat(o, rxBuffer);
            o += 4;
            currentData.mag[2] = convertRxbytesToFloat(o, rxBuffer);
            o += 4;
        }

        if (angularVelEnable) {
            currentData.angVel[0] = convertRxbytesToFloat(o, rxBuffer) * r2d;
            o += 4;
            currentData.angVel[1] = convertRxbytesToFloat(o, rxBuffer) * r2d;
            o += 4;
            currentData.angVel[2] = convertRxbytesToFloat(o, rxBuffer) * r2d;
            o += 4;
        }

        if (quaternionEnable) {
            currentData.quat[0] = convertRxbytesToFloat(o, rxBuffer);
            o += 4;
            currentData.quat[1] = convertRxbytesToFloat(o, rxBuffer);
            o += 4;
            currentData.quat[2] = convertRxbytesToFloat(o, rxBuffer);
            o += 4;
            currentData.quat[3] = convertRxbytesToFloat(o, rxBuffer);
            o += 4;
        }

        if (eulerAngleEnable) {
            currentData.euler[0] = convertRxbytesToFloat(o, rxBuffer) * r2d;
            o += 4;
            currentData.euler[1] = convertRxbytesToFloat(o, rxBuffer) * r2d;
            o += 4;
            currentData.euler[2] = convertRxbytesToFloat(o, rxBuffer) * r2d;
            o += 4;
        }

        if (linAccEnable) {
            currentData.linAcc[0] = convertRxbytesToFloat(o, rxBuffer);
            o += 4;
            currentData.linAcc[1] = convertRxbytesToFloat(o, rxBuffer);
            o += 4;
            currentData.linAcc[2] = convertRxbytesToFloat(o, rxBuffer);
            o += 4;
        }

        if (pressureEnable) {
            currentData.pressure = convertRxbytesToFloat(o, rxBuffer);
            o += 4;
        }

        if (altitudeEnable) {
            currentData.altitude = convertRxbytesToFloat(o, rxBuffer);
            o += 4;
        }

        if (temperatureEnable) {
            currentData.temperature = convertRxbytesToFloat(o, rxBuffer);
            o += 4;
        }

        if (heaveEnable) {
            currentData.heave = convertRxbytesToFloat(o, rxBuffer);
            o += 4;
        }

        synchronized (dataQueue) {
            if (dataQueue.size() < DATA_QUEUE_SIZE)
                dataQueue.addFirst(new LpmsB2SensorData(currentData));
            else {
                dataQueue.removeLast();
                dataQueue.addFirst(new LpmsB2SensorData(currentData));
            }

            if (isRecording && tmpFileWriter != null) {
                if (!initMap.containsKey(currentData.imuId)) {
                    initMap.put(currentData.imuId, false);
                }
                if (!initMap.get(currentData.imuId)) {
                    if (currentData.timestamp > 0.5) {
                        newDataFlag = false;
                        return;
                    } else {
                        initMap.put(currentData.imuId, true);
                    }
                }
                records.add(new LpmsB2SensorData(currentData));
                try {
                    tmpFileWriter.append(String.valueOf(currentData.imuId)).append(",")
                            .append(String.valueOf(currentData.localtime)).append(",")
                            .append(String.valueOf(currentData.timestamp)).append(",")
                            .append(String.valueOf(currentData.frameNumber)).append(",")
                            .append(String.valueOf(currentData.acc[0])).append(",")
                            .append(String.valueOf(currentData.acc[1])).append(",")
                            .append(String.valueOf(currentData.acc[2])).append(",")
                            .append(String.valueOf(currentData.gyr[0])).append(",")
                            .append(String.valueOf(currentData.gyr[1])).append(",")
                            .append(String.valueOf(currentData.gyr[2])).append(",")
                            .append(String.valueOf(currentData.mag[0])).append(",")
                            .append(String.valueOf(currentData.mag[1])).append(",")
                            .append(String.valueOf(currentData.mag[2])).append(",")
                            .append(String.valueOf(currentData.euler[0])).append(",")
                            .append(String.valueOf(currentData.euler[1])).append(",")
                            .append(String.valueOf(currentData.euler[2])).append(",")
                            .append(String.valueOf(currentData.quat[0])).append(",")
                            .append(String.valueOf(currentData.quat[1])).append(",")
                            .append(String.valueOf(currentData.quat[2])).append(",")
                            .append(String.valueOf(currentData.quat[3])).append(",")
                            .append(String.valueOf(currentData.linAcc[0])).append(",")
                            .append(String.valueOf(currentData.linAcc[1])).append(",")
                            .append(String.valueOf(currentData.linAcc[2])).append(",")
                            .append(String.valueOf(currentData.pressure)).append(",")
                            .append(String.valueOf(currentData.altitude)).append(",")
                            .append(String.valueOf(currentData.temperature)).append(",")
                            .append(String.valueOf(currentData.batteryLevel)).append("\n");
                } catch (IOException e) {
                    e.printStackTrace();
                }
            }
        }

        newDataFlag = true;
    }

    void parseSensorData16Bit() {
        int o = 0;
        float r2d = 57.2958f;

        currentData.imuId = imuId;
        currentData.timestamp = (float) convertRxbytesToInt(0, rxBuffer) * 0.0025f;
        currentData.localtime = System.currentTimeMillis();
        o += 4;
        currentData.frameNumber = frameCounter;
        frameCounter++;

        if (gyrEnable) {
            for (int i = 0; i < 3; ++i) {
                currentData.gyr[i] = (float) ((short) (((rxBuffer[o + 1]) << 8) | (rxBuffer[o + 0] & 0xff))) / 1000.0f * r2d;
                o += 2;
            }
            //LogUtil.d(currentData.gyr[0]+" "+currentData.gyr[1]+" "+currentData.gyr[2]);
        }

        if (accEnable) {
            for (int i = 0; i < 3; ++i) {
                currentData.acc[i] = (float) ((short) (((rxBuffer[o + 1]) << 8) | (rxBuffer[o + 0] & 0xff))) / 1000.0f;
                o += 2;
            }
        }

        if (magEnable) {
            for (int i = 0; i < 3; ++i) {
                currentData.mag[i] = (float) ((short) (((rxBuffer[o + 1]) << 8) | (rxBuffer[o + 0] & 0xff))) / 100.0f;
                o += 2;
            }
        }

        if (angularVelEnable) {
            for (int i = 0; i < 3; ++i) {
                currentData.angVel[i] = (float) ((short) (((rxBuffer[o + 1]) << 8) | (rxBuffer[o + 0] & 0xff))) / 1000.0f * r2d;
                o += 2;
            }
        }

        if (quaternionEnable) {
            for (int i = 0; i < 4; ++i) {
                currentData.quat[i] = (float) ((short) (((rxBuffer[o + 1]) << 8) | (rxBuffer[o + 0] & 0xff))) / 1000.0f;
                o += 2;
            }
        }

        if (eulerAngleEnable) {
            for (int i = 0; i < 3; ++i) {
                currentData.euler[i] = (float) ((short) (((rxBuffer[o + 1]) << 8) | (rxBuffer[o + 0] & 0xff))) / 1000.0f * r2d;
                o += 2;
            }
        }

        if (linAccEnable) {
            for (int i = 0; i < 3; ++i) {
                currentData.linAcc[i] = (float) ((short) (((rxBuffer[o + 1]) << 8) | (rxBuffer[o + 0] & 0xff))) / 1000.0f;
                o += 2;
            }
        }

        if (pressureEnable) {
            currentData.pressure = (float) ((short) (((rxBuffer[o + 1]) << 8) | (rxBuffer[o + 0] & 0xff))) / 100.0f;
            o += 2;
        }

        if (altitudeEnable) {
            currentData.altitude = (float) ((short) (((rxBuffer[o + 1]) << 8) | (rxBuffer[o + 0] & 0xff))) / 10.0f;
            o += 2;
        }

        if (temperatureEnable) {
            currentData.temperature = (float) ((short) (((rxBuffer[o + 1]) << 8) | (rxBuffer[o + 0] & 0xff))) / 100.0f;
            o += 2;
        }

        if (heaveEnable) {
            currentData.heave = (float) ((short) (((rxBuffer[o + 1]) << 8) | (rxBuffer[o + 0] & 0xff))) / 100.0f;
            o += 2;
        }

        synchronized (dataQueue) {
            if (dataQueue.size() < DATA_QUEUE_SIZE)
                dataQueue.addFirst(new LpmsB2SensorData(currentData));
            else {
                dataQueue.removeLast();
                dataQueue.addFirst(new LpmsB2SensorData(currentData));
            }
        }

        newDataFlag = true;
    }

    void parseFunction() {
        switch (currentFunction) {
            case REPLY_ACK:
                //LogUtil.d("Received ACK");
                waitForAck = false;
                break;

            case REPLY_NACK:
                //LogUtil.d("Received NACK");
                waitForAck = false;
                break;

            case GET_CONFIG:
                configurationRegister = convertRxbytesToInt(0, rxBuffer);
                //LogUtil.d("GET_CONFIG: " + configurationRegister);
                configurationRegisterReady = true;
                waitForData = false;
                break;

            case GET_STATUS:
                waitForData = false;
                break;

            case GET_SENSOR_DATA:
                if ((configurationRegister & LPMS_LPBUS_DATA_MODE_16BIT_ENABLED) != 0) {
                    parseSensorData16Bit();
                } else {
                    parseSensorData();
                }
                waitForData = false;
                break;

            case GET_IMU_ID:
                imuId = convertRxbytesToInt(0, rxBuffer);
                waitForData = false;
                break;

            case GET_GYR_RANGE:
                gyrRange = convertRxbytesToInt(0, rxBuffer);
                waitForData = false;
                break;

            case GET_ACC_RANGE:
                accRange = convertRxbytesToInt(0, rxBuffer);
                waitForData = false;
                break;

            case GET_MAG_RANGE:
                magRange = convertRxbytesToInt(0, rxBuffer);
                waitForData = false;
                break;

            case GET_FILTER_MODE:
                filterMode = convertRxbytesToInt(0, rxBuffer);
                waitForData = false;
                break;

            case GET_BATTERY_LEVEL:
                currentData.batteryLevel = convertRxbytesToFloat(0, rxBuffer);
//                LogUtil.d("GET_BATTERY_LEVEL " +  currentData.batteryLevel);
                waitForData = false;
                break;

            case GET_CHARGING_STATUS:
                currentData.chargingStatus = convertRxbytesToInt(0, rxBuffer);
                //LogUtil.d("GET_CHARGING_STATUS ");
                waitForData = false;
                break;

            case GET_BATTERY_VOLTAGE:
                currentData.batteryVoltage = convertRxbytesToFloat(0, rxBuffer);
//                LogUtil.d("GET_BATTERY_VOLTAGE " + currentData.batteryVoltage);
                waitForData = false;
                break;

            case GET_SERIAL_NUMBER:
                serialNumber = convertRxbytesToString(24, rxBuffer);
                //LogUtil.d(serialNumber);
                serialNumberReady = true;
                waitForData = false;
                break;

            case GET_DEVICE_NAME:
                deviceName = convertRxbytesToString(16, rxBuffer);
                //LogUtil.d(deviceName);
                deviceNameReady = true;
                waitForData = false;
                break;

            case GET_FIRMWARE_INFO:
                firmwareInfo = convertRxbytesToString(16, rxBuffer);
                //LogUtil.d(firmwareInfo);
                firmwareInfoReady = true;
                waitForData = false;
                break;

            case GET_FIRMWARE_VERSION:
                int vmajor = convertRxbytesToInt(8, rxBuffer);
                int vminor = convertRxbytesToInt(4, rxBuffer);
                int vbuild = convertRxbytesToInt(0, rxBuffer);
                firmwareVersion = Integer.toString(vmajor) + "." + Integer.toString(vminor) + "." + Integer.toString(vbuild);
                //LogUtil.d( "FW Ver: " +  firmwareVersion);
                waitForData = false;
                break;

            case GET_PING:
                float mT;
                mT = (float) convertRxbytesToInt(0, rxBuffer) * 0.0025f;
                //LogUtil.d("GET_PING: " +  mT );
                waitForData = false;
                break;

            case START_SYNC:
                //LogUtil.d("START_SYNC");
                waitForAck = false;
                break;

            case STOP_SYNC:
                //LogUtil.d("STOP_SYNC");
                waitForAck = false;
                break;

            case SET_TRANSMIT_DATA:
                waitForData = false;
                break;

            case GET_TEMPERATURE:
                currentData.temperature = convertRxbytesToFloat(0, rxBuffer);
                //LogUtil.d("GET_TEMPERATURE");
                waitForData = false;
                break;
        }

        //waitForAck = false;
        // waitForData = false;
    }

    void parse() {
        int lrcReceived = 0;
        //String s="";
        for (int i = 0; i < nBytes; i++) {
            b = rawRxBuffer[i];
            //s += Integer.toHexString(b& 0xFF) + " " ;
            switch (rxState) {
                case PACKET_END:
                    if (b == 0x3a) {
                        rxState = PACKET_ADDRESS0;
                    }
                    break;

                case PACKET_ADDRESS0:
                    inBytes[0] = b;
                    rxState = PACKET_ADDRESS1;
                    break;

                case PACKET_ADDRESS1:
                    inBytes[1] = b;
                    currentAddress = convertRxbytesToInt16(0, inBytes);
                    imuId = currentAddress;
                    rxState = PACKET_FUNCTION0;
                    break;

                case PACKET_FUNCTION0:
                    inBytes[0] = b;
                    rxState = PACKET_FUNCTION1;
                    break;

                case PACKET_FUNCTION1:
                    inBytes[1] = b;
                    currentFunction = convertRxbytesToInt16(0, inBytes);
                    rxState = PACKET_LENGTH0;
                    break;

                case PACKET_LENGTH0:
                    inBytes[0] = b;
                    rxState = PACKET_LENGTH1;
                    break;

                case PACKET_LENGTH1:
                    inBytes[1] = b;
                    currentLength = convertRxbytesToInt16(0, inBytes);
                    rxState = PACKET_RAW_DATA;
                    rxIndex = 0;
                    break;

                case PACKET_RAW_DATA:
                    if (rxIndex == currentLength) {
                        lrcCheck = (currentAddress & 0xffff) + (currentFunction & 0xffff) + (currentLength & 0xffff);
                        for (int j = 0; j < currentLength; j++) {
                            if (j < MAX_BUFFER) {
                                lrcCheck += (int) rxBuffer[j] & 0xff;
                            } else break;

                        }

                        inBytes[0] = b;
                        rxState = PACKET_LRC_CHECK1;
                    } else {
                        if (rxIndex < MAX_BUFFER) {

                            rxBuffer[rxIndex] = b;
                            rxIndex++;
                        } else break;
                    }
                    break;

                case PACKET_LRC_CHECK1:
                    inBytes[1] = b;

                    lrcReceived = convertRxbytesToInt16(0, inBytes);
                    lrcCheck = lrcCheck & 0xffff;

                    if (lrcReceived == lrcCheck) {
                        parseFunction();
                    }

                    rxState = PACKET_END;
                    break;

                default:
                    rxState = PACKET_END;
                    break;
            }
        }
        // LogUtil.d("[LpmsBThread] Received: " + s);
    }

    void sendData(int function, int length) {
        int txLrcCheck;

        txBuffer[0] = 0x3a;
        convertInt16ToTxbytes(imuId, 1, txBuffer);
        convertInt16ToTxbytes(function, 3, txBuffer);
        convertInt16ToTxbytes(length, 5, txBuffer);

        for (int i = 0; i < length; ++i) {
            txBuffer[7 + i] = rawTxData[i];
        }

        txLrcCheck = (imuId & 0xffff) + (function & 0xffff) + (length & 0xffff);

        for (int j = 0; j < length; j++) {
            txLrcCheck += (int) rawTxData[j] & 0xff;
        }

        convertInt16ToTxbytes(txLrcCheck, 7 + length, txBuffer);
        txBuffer[9 + length] = 0x0d;
        txBuffer[10 + length] = 0x0a;

        String s = "";

        for (int i = 0; i < 11 + length; i++) {
            s += Integer.toHexString(txBuffer[i] & 0xFF) + " ";
        }
        //LogUtil.d("[LpmsBThread] Sending: " + s);

        try {
            // LogUtil.d("[LpmsBThread] Sending data");
            mOutStream.write(txBuffer, 0, length + 11);
        } catch (Exception e) {
            //LogUtil.d("[LpmsBThread] Error while sending data");
            e.printStackTrace();
            // throw new RuntimeException(e);
        }
    }

    void sendAck() {
        sendData(REPLY_ACK, 0);
    }

    void sendNack() {
        sendData(REPLY_NACK, 0);
    }

    float convertRxbytesToFloat(int offset, byte buffer[]) {
        int l = buffer[offset + 0];
        l &= 0xff;
        l |= ((long) buffer[offset + 1] << 8);
        l &= 0xffff;
        l |= ((long) buffer[offset + 2] << 16);
        l &= 0xffffff;
        l |= ((long) buffer[offset + 3] << 24);
        return Float.intBitsToFloat(l);
    }

    int convertRxbytesToInt(int offset, byte buffer[]) {
        int v = (int) ((buffer[offset] & 0xFF)
                | ((buffer[offset + 1] & 0xFF) << 8)
                | ((buffer[offset + 2] & 0xFF) << 16)
                | ((buffer[offset + 3] & 0xFF) << 24));
        return v;
    }

    int convertRxbytesToInt16(int offset, byte buffer[]) {
        int v = (int) ((buffer[offset] & 0xFF)
                | ((buffer[offset + 1] << 8) & 0xFF00));
        return v;
    }

    String convertRxbytesToString(int length, byte buffer[]) {
        byte[] t = new byte[length];
        for (int i = 0; i < length; i++) {
            t[i] = buffer[i];
        }

        String decodedString = new String(t).trim();
        return decodedString;
    }

    void convertIntToTxbytes(int v, int offset, byte buffer[]) {
        byte[] t = ByteBuffer.allocate(4).putInt(v).array();

        for (int i = 0; i < 4; i++) {
            buffer[3 - i + offset] = t[i];
        }
    }

    void convertInt16ToTxbytes(int v, int offset, byte buffer[]) {
        byte[] t = ByteBuffer.allocate(2).putShort((short) v).array();

        for (int i = 0; i < 2; i++) {
            buffer[1 - i + offset] = t[i];
        }
    }

    void convertFloatToTxbytes(float f, int offset, byte buffer[]) {
        int v = Float.floatToIntBits(f);
        byte[] t = ByteBuffer.allocate(4).putInt(v).array();

        for (int i = 0; i < 4; i++) {
            buffer[3 - i + offset] = t[i];
        }
    }

    /*
    public boolean startLogging() {
        if (connectionStatus == SENSOR_STATUS_DISCONNECTED) {
            dl.setStatusMesg("No sensor connected");
            return false;
        }
        if (dl.startLogging()) {
            resetTimestamp();
            return true;
        }
        return false;
    }

    public boolean stopLogging() {
        return dl.stopLogging();
    }

    public boolean isLogging() {
        return dl.isLogging();
    }

    public String getLoggerStatusMesg() {
        return dl.getStatusMesg();
    }

    public String getOutputFilename() {
       return dl.getOutputFilename();
   }
*/
    private void _waitForAckLoop() {
        int timeout = 0;
        while (timeout++ < 30 && waitForAck) {
            try {
                Thread.sleep(PARAMETER_SET_DELAY);
            } catch (Exception e) {
                e.printStackTrace();
                // throw new RuntimeException(e);
            }
        }
    }

    private void _waitForDataLoop() {
        int timeout = 0;
        while (timeout++ < 30 && waitForData) {
            try {
                Thread.sleep(PARAMETER_SET_DELAY);
            } catch (Exception e) {
                e.printStackTrace();
                // throw new RuntimeException(e);
            }
        }
    }

    private void _getSensorSettings() {
        _getConfig();
        _getGyroRange();
        _getAccRange();
        _getMagRange();
        _getFilterMode();
        printConfig();

    }

    private void _getConfig() {
        configurationRegisterReady = false;
        lpbusSetNone(GET_CONFIG);
        //LogUtil.d("Send GET_CONFIG");
        int timeout = 0;
        while (timeout++ < 30 && !configurationRegisterReady) {
            try {
                Thread.sleep(PARAMETER_SET_DELAY);
            } catch (Exception e) {
                e.printStackTrace();
                // new RuntimeException(e);
            }
        }
        // Parse configuration
        parseConfig(configurationRegister);
    }


    private void parseConfig(int config) {
        //LogUtil.d("config: " + config);

        // Stream frequency
        if ((configurationRegister & LPMS_STREAM_FREQ_MASK) == LPMS_STREAM_FREQ_5HZ_ENABLED)
            streamingFrequency = STREAM_FREQ_5HZ;
        else if ((configurationRegister & LPMS_STREAM_FREQ_MASK) == LPMS_STREAM_FREQ_10HZ_ENABLED)
            streamingFrequency = STREAM_FREQ_10HZ;
        else if ((configurationRegister & LPMS_STREAM_FREQ_MASK) == LPMS_STREAM_FREQ_25HZ_ENABLED)
            streamingFrequency = STREAM_FREQ_25HZ;
        else if ((configurationRegister & LPMS_STREAM_FREQ_MASK) == LPMS_STREAM_FREQ_50HZ_ENABLED)
            streamingFrequency = STREAM_FREQ_50HZ;
        else if ((configurationRegister & LPMS_STREAM_FREQ_MASK) == LPMS_STREAM_FREQ_100HZ_ENABLED)
            streamingFrequency = STREAM_FREQ_100HZ;
        else if ((configurationRegister & LPMS_STREAM_FREQ_MASK) == LPMS_STREAM_FREQ_200HZ_ENABLED)
            streamingFrequency = STREAM_FREQ_200HZ;
        else if ((configurationRegister & LPMS_STREAM_FREQ_MASK) == LPMS_STREAM_FREQ_400HZ_ENABLED)
            streamingFrequency = STREAM_FREQ_400HZ;

        if ((config & LPMS_GYR_RAW_OUTPUT_ENABLED) != 0) {
            gyrEnable = true;
            //LogUtil.d("GYRO ENABLED");
        } else {
            gyrEnable = false;
            //LogUtil.d("GYRO DISABLED");
        }
        if ((config & LPMS_ACC_RAW_OUTPUT_ENABLED) != 0) {
            accEnable = true;
            //LogUtil.d("ACC ENABLED");
        } else {
            accEnable = false;
            //LogUtil.d("ACC DISABLED");
        }
        if ((config & LPMS_MAG_RAW_OUTPUT_ENABLED) != 0) {
            magEnable = true;
            //LogUtil.d("MAG ENABLED");
        } else {
            magEnable = false;
            //LogUtil.d("MAG DISABLED");
        }
        if ((config & LPMS_ANGULAR_VELOCITY_OUTPUT_ENABLED) != 0) {
            angularVelEnable = true;
            //LogUtil.d("AngVel ENABLED");
        } else {
            angularVelEnable = false;
            //LogUtil.d("AngVel DISABLED");
        }
        if ((config & LPMS_QUAT_OUTPUT_ENABLED) != 0) {
            quaternionEnable = true;
            //LogUtil.d("QUAT ENABLED");
        } else {
            quaternionEnable = false;
            //LogUtil.d("QUAT DISABLED");
        }
        if ((config & LPMS_EULER_OUTPUT_ENABLED) != 0) {
            eulerAngleEnable = true;
            //LogUtil.d("EULER ENABLED");
        } else {
            eulerAngleEnable = false;
            //LogUtil.d("EULER DISABLED");
        }
        if ((config & LPMS_LINACC_OUTPUT_ENABLED) != 0) {
            linAccEnable = true;
            //LogUtil.d("LINACC ENABLED");
        } else {
            linAccEnable = false;
            //LogUtil.d("LINACC DISABLED");
        }
        if ((config & LPMS_PRESSURE_OUTPUT_ENABLED) != 0) {
            pressureEnable = true;
            //LogUtil.d("PRESSURE ENABLED");
        } else {
            pressureEnable = false;
            //LogUtil.d("PRESSURE DISABLED");
        }

        if ((config & LPMS_TEMPERATURE_OUTPUT_ENABLED) != 0) {
            temperatureEnable = true;
            //LogUtil.d("PRESSURE ENABLED");
        } else {
            temperatureEnable = false;
            //LogUtil.d("PRESSURE DISABLED");
        }

        if ((config & LPMS_ALTITUDE_OUTPUT_ENABLED) != 0) {
            altitudeEnable = true;
            //LogUtil.d("PRESSURE ENABLED");
        } else {
            altitudeEnable = false;
            //LogUtil.d("PRESSURE DISABLED");
        }
        if ((config & LPMS_HEAVEMOTION_OUTPUT_ENABLED) != 0) {
            heaveEnable = true;
            //LogUtil.d("heave ENABLED");
        } else {
            heaveEnable = false;
            //LogUtil.d("heave DISABLED");
        }

        if ((config & LPMS_LPBUS_DATA_MODE_16BIT_ENABLED) != 0) {
            sixteenBitDataEnable = true;
            //LogUtil.d("16 bit ENABLED");
        } else {
            sixteenBitDataEnable = false;
            //LogUtil.d("16 bit DISABLED");
        }

        //LogUtil.d("Stream freq: " + streamingFrequency);
    }

    private void _getGyroRange() {
        waitForData = true;
        lpbusSetNone(GET_GYR_RANGE);
        //LogUtil.d("Send GET_GYR_RANGE");
        _waitForDataLoop();
        //LogUtil.d("Gyro range: " + gyrRange + "dps");
    }

    private void _getAccRange() {
        waitForData = true;
        lpbusSetNone(GET_ACC_RANGE);
        //LogUtil.d("Send GET_ACC_RANGE");
        _waitForDataLoop();
        //LogUtil.d("Acc range: " + accRange + "g");
    }

    private void _getMagRange() {
        waitForData = true;
        lpbusSetNone(GET_MAG_RANGE);
        //LogUtil.d("Send GET_MAG_RANGE");
        _waitForDataLoop();
        //LogUtil.d("Mag range: " + magRange + "mT");
    }

    private void _getFilterMode() {
        waitForData = true;
        lpbusSetNone(GET_FILTER_MODE);
        //LogUtil.d("Send GET_FILTER_MODE");
        _waitForDataLoop();
        //LogUtil.d("Filter mode: " + filterMode);
    }

    private void _getSerialNumber() {
        serialNumberReady = false;
        serialNumber = "";
        lpbusSetNone(GET_SERIAL_NUMBER);
        int timeout = 0;
        while (timeout++ < 30 && !serialNumberReady) {
            try {
                Thread.sleep(PARAMETER_SET_DELAY);
            } catch (InterruptedException e) {

                e.printStackTrace();
            }
        }
    }

    private void _getDeviceName() {
        deviceNameReady = false;
        deviceName = "";
        lpbusSetNone(GET_DEVICE_NAME);
        int timeout = 0;
        while (timeout++ < 30 && !deviceNameReady) {
            try {
                Thread.sleep(PARAMETER_SET_DELAY);
            } catch (InterruptedException e) {

                e.printStackTrace();
            }
        }
    }

    private void _getFirmwareInfo() {
        firmwareInfoReady = false;
        firmwareInfo = "";
        lpbusSetNone(GET_FIRMWARE_INFO);
        int timeout = 0;
        while (timeout++ < 30 && !firmwareInfoReady) {
            try {
                Thread.sleep(PARAMETER_SET_DELAY);
            } catch (InterruptedException e) {
                e.printStackTrace();
                //throw new RuntimeException(e);
            }
        }
    }

    private void _saveParameters() {
        waitForAck = true;
        lpbusSetNone(WRITE_REGISTERS);
        _waitForAckLoop();
    }

    private void _setTransmissionData() {
        new Thread(new Thread() {
            public void run() {
                boolean b = isStreamMode;
                setCommandMode();
                waitForAck = true;
                lpbusSetInt32(SET_TRANSMIT_DATA, configurationRegister);
                _waitForAckLoop();
                _getSensorSettings();
                _saveParameters();
                if (b)
                    setStreamingMode();
            }
        }).start();
    }

    private boolean assertConnected() {
        if (connectionStatus == SENSOR_STATUS_CONNECTED)
            return true;
        return false;
    }


    private void printConfig() {
        //LogUtil.d("config: " + config);
        LogUtil.d("SN: " + serialNumber);
        LogUtil.d("FW: " + firmwareInfo);
        LogUtil.d("DN: " + deviceName);
        LogUtil.d("ImuId: " + imuId);
        LogUtil.d("StreamFreq: " + streamingFrequency);
        LogUtil.d("Gyro: " + gyrRange);
        LogUtil.d("Acc: " + accRange);
        LogUtil.d("Mag: " + magRange);

        if (gyrEnable) {
            LogUtil.d("GYRO ENABLED");
        } else {
            LogUtil.d("GYRO DISABLED");
        }
        if (accEnable) {
            LogUtil.d("ACC ENABLED");
        } else {
            LogUtil.d("ACC DISABLED");
        }
        if (magEnable) {
            LogUtil.d("MAG ENABLED");
        } else {
            LogUtil.d("MAG DISABLED");
        }
        if (angularVelEnable) {
            LogUtil.d("AngVel ENABLED");
        } else {
            LogUtil.d("AngVel DISABLED");
        }
        if (quaternionEnable) {
            LogUtil.d("QUAT ENABLED");
        } else {
            LogUtil.d("QUAT DISABLED");
        }
        if (eulerAngleEnable) {
            LogUtil.d("EULER ENABLED");
        } else {
            LogUtil.d("EULER DISABLED");
        }
        if (linAccEnable) {
            LogUtil.d("LINACC ENABLED");
        } else {
            LogUtil.d("LINACC DISABLED");
        }
        if (pressureEnable) {
            LogUtil.d("PRESSURE ENABLED");
        } else {
            LogUtil.d("PRESSURE DISABLED");
        }
        if (altitudeEnable) {
            LogUtil.d("ALTITUDE ENABLED");
        } else {
            LogUtil.d("ALTITUDE DISABLED");
        }
        if (temperatureEnable) {
            LogUtil.d("TEMPERATURE ENABLED");
        } else {
            LogUtil.d("TEMPERATURE DISABLED");
        }

        if (heaveEnable) {
            LogUtil.d("heave ENABLED");
        } else {
            LogUtil.d("heave DISABLED");
        }

        if (sixteenBitDataEnable) {
            LogUtil.d("16 bit ENABLED");
        } else {
            LogUtil.d("16 bit DISABLED");
        }

    }

    public static void startRecording(Context context) {
        LpmsB2Sensor.context = context;
        isRecording = true;
        LogUtil.w("isRecording="+isRecording);
        tmpFileWriter = FileUtil.getDataSetTempFileWriter(context);
        try {
            if (tmpFileWriter != null) {
                tmpFileWriter.append("lpmsb2\n");
            } else {
                throw new Exception("get tmpFileWriter failed.");
            }
        } catch (Exception e) {
            e.printStackTrace();
        }
    }

    public static void stopRecording() {
        isRecording = false;
        LogUtil.w("isRecording="+isRecording);
        try {
            if (tmpFileWriter != null) {
                tmpFileWriter.close();
                tmpFileWriter = null;
            }
        } catch (IOException e) {
            e.printStackTrace();
        }
    }

    public static boolean isRecording() {
        LogUtil.w("isRecording="+isRecording);
        return isRecording;
    }

    public class ClientReadThread implements Runnable {

        public void run() {
            while (mSocket.isConnected()) {
                try {
                    nBytes = mInStream.read(rawRxBuffer);
                } catch (Exception e) {
                    break;
                }

                parse();

            }

        }
    }
}
